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Symmetry-preserving observers 



Silvere Bonnabel, Philippe Martin and Pierre Rouchon 



Abstract 

This paper presents three non-linear observers for three examples of engineering interest: a non- 
holonomic car, a chemical reactor, and an inertial navigation system. For each example, the design is 
based on physical symmetries. This motivates the theoretical development of invariant observers, i.e, 
symmetry-preserving observers. We consider an observer to consist of a copy of the system equation and 
a correction term, and we propose a constructive method (based on the Cartan moving-frame method) 
to find all the symmetry-preserving correction terms. The construction relies on an invariant frame (a 
classical notion) and on an invariant output-error, a less standard notion precisely defined here. For each 
example, the convergence analysis relies on the use of invariant state-errors, a symmetry-preserving way 
to define the estimation error. 

Index Terms 

Nonlinear observer, invariants, symmetry, moving frame, inertial navigation, chemical reactor. 

I. Introduction 

Symmetries have been used in control theory for feedback design and optimal control, see for 
instance [9], [10], [18], [19], [14], [20] but much less for observer design [3], [2], [13], [12], 
[11]. In this paper we use symmetries for observer design and we develop a theory of invariant 
observers. This theory is motivated by three non-linear examples of engineering interest: a non- 
holonomic car, an exothermic chemical reactor and a velocity-aided inertial navigation system. 
In each case the symmetries have an obvious physical interpretation. For the first example we 
propose a non-linear observer which converges for any initial condition except one (theorem H]). 
For the second, we design a non-linear globally convergent observer (theorem |5]). For the third, 
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the observer is locally convergent around any system trajectory. Moreover the global behavior 
is independent of the system trajectory (theorem [6]). This theory may be applied to many other 
systems such as those treated in [8], [12], [11] where the invariance relative to the choice of the 
reference 3D-frame is exploited in observer design and convergence analysis. 

The theoretical contribution of the paper is the following: for the smooth system with state x, 
input u and output y, invariance under the action of a Lie group G is defined and corresponds to 
a separate action of G on the state-space, on the input-space and on the output space. Invariance 
means that the dynamics jj^x = f(x,u) and the output map y = h(x) remain unchanged by 
a change of state, input, and output coordinates corresponding to the action of G. We define 
invariance for an asymptotic Luenberger nonlinear observer under the action G similarly, where 
the group acts also on the estimated space and the estimated output in a similar way. When the 
group dimension does not exceed the state dimension we propose (theorem Q]) a constructive 
design of the invariant observer. This construction is based on an invariant frame and an invariant 
output-error. Such invariant output-errors (definition [8]) are introduced here for the first time and 
can be computed via Cartan's moving frame method (theorem©. We show how to transform a 
locally convergent asymptotic observer around an equilibrium point into an invariant one with 
the same first order approximation. To deal with convergence issues, we introduce invariant 
state-errors. The three examples show that these state-errors play key role in the convergence 
analysis. 

The content of this paper is as follows: in section [III we define invariant systems and invariant 
pre-observers. The general form of an invariant pre-observer is given in theorem \T\ it relies 
on invariant output errors and invariant vector fields. Their explicit construction relies on the 
moving frame method [16], which is summarized in subsection III-Cl Around an equilibrium, 
we show it is always possible to build an invariant observer whose linear tangent approximation 
is any linear asymptotic observer of the Luenberger type. To study the convergence, we define 
invariant state error. It is a way of defining the error equation so that it preserves the symmetries, 
while the usual x — x does not in general. It obeys a differential system where only the invariant 
part of the system trajectory appears (theorem [3]). This property reduces the dimension of the 
convergence problem and appears to play a crucial role in the examples. In section [Till we study 
in detail three physical examples. 

A summary of the state of the art results on symmetries of dynamic systems can be found in 
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the monograph [17]. The notion of invariant observer and invariant output error can be found 
in [2], [1]. Other preliminary results presented in this paper can be found in [7], [6], [5]. 

II. Invariant systems, observers and errors 

A. Invariant systems and equivariant outputs 

Definition 1: Let G be a Lie Group with identity e and S an open set (or more generally a 
manifold). A transformation group {4>g) g eG on E is a smooth map 

(^)gGxSk^)gS 

such that: 

. UO = f for all £ 

• 092 (09i 

Notice 4> g is by construction a diffeomorphism on E for all g. The transformation group is 
local if (f> g (£) is defined only when g lies sufficiently near e. In this case the transformation 
law 92 = 0929i (0 is imposed only when it makes sense. All the results of the paper 

being local, since based on constant rank assumptions, we consider in this section only local 
transformation groups acting on open sets. When we say "for all g" we thus mean "for all g 
sufficiently near the identity e of G"; in the same way "for all £" usually means "for all generic £ 
in E". We systematically use these stylistic shortcuts in order to improve readability. 

Consider now the smooth system 

^x = f{x,u) (1) 

y = h(x,u) (2) 

where x belongs to an open subset X C M™, u to an open subset U C K m and y to an open 
subset y CW, p <n. 

We assume the signals u(t),y(t) known (y is measured, and u is measured or known - control 
input, measured perturbation, constant parameter). 

Consider also the local group of transformations on X x U defined by 

(X,U) = ( Vg (x),^ g (u)), (3) 
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where <p g and ipg are local diffeomorphisms. Notice cp g acts on X and ty g acts on U. u can also 
denote the time t but in this case ip g is the identity function. The two following definitions are 
inspired from [14]. 

Definition 2: The system J-x = f(x, u) is G-invariant if f (<f g (x),ip g (u)) = Dip g (x) ■ f(x, u) 
for all g, x, u. 

The property also reads j^ t X = f(X, U), i.e., the system remains unchanged under the transfor- 
mation ©. 

Definition 3: The output y = h(x,u) is G-equivariant if there exists a transformation group 
(Qg)g&G on y such that h((p g (x), ip g (u)) = Q g (h(x,uj) for all g,x,u. 

With (X,U) = ((p g (x),il> g (u)) and Y = g g (y), the definition means Y = h(X,U). The two 
previous definitions can be illustrated by the commutative diagram 

TX 



f 

X x U 

h 

y 



XxU 



y 



B. Basic assumptions 

From now on we consider a G-invariant system jjrx = f(x,u) with a G-equivariant output 
y = h(x, u). We let r < n be the dimension of the group G. We systematically assume for each 
x, the mapping g i— ► v^gl^) i s full rank. 



C. The moving frame method, invariant vector fields, base and fiber coordinates 

1) Moving frame method: This paragraph is independent of the rest of the paper. It is a recap 
of the general presentation of [16, theorem 8.25]). Take a r-dimensional transformation group 
G acting on E C R s via the diffeomorphisms (0 9 ) ge c such that r < s. We suppose that d g 4> g 
has full rank r := dimG at the point (e, £°) 6GxS. We can then split 4> g into ((f) g , (j) b g ) with 
respectively r and s — r components so that (j) g is invertible with respect to g around (e, 
The normalization equations are obtained setting 
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with c a constant in the range of <p a . The implicit function theorem ensures the existence of the 
local solution g = 7(£) (the map 7 : E — > G is known as the moving frame). Thus 

One can also say {^"(0 — c ) defines a coordinate cross-section to the orbits, and g = 7^) is the 
unique group element that maps £ to the cross-section. Finally, we get a complete set J of s — r 
functionally independent invariants by substituting g — 7^) into the remaining transformation 
rules, 

■/(£):= # (€ )(0- 

The invariance property means J(<j> g (£)) = J(Q for all To prove it let ( = 4> 9 {i). We 
have tylMcfogit)) = ^7(0 (0 = c - But the S rou P composition implies ^^(^(O) = 

^(^(o)ff(O- Thus ^(0)9(0 = c which p roves b y unicit y of 7(0 e g 

7(0,(0)5 = 7(0 (4) 
which is the main property (equivariance) of the moving frame that proves indeed 

J(M0) = <^ g (o)(^(0) = <(0 9 ( O)9 (O = 7(O (O = m 

Moreover any other local invariant J', i.e, any real-valued function J' which verifies J'(0s(O) = 
J'(0 f° r a fi can be written as a function of the complete set of invariants: J' — H(J). 

2) Invariant vector fields and invariant frame: The moving frame method allows us to build 
invariant frames, which play a role in the construction of invariant observers. 

Definition 4: A vector field w on X is said to be G-invariant if the system -^x = w(x) is 
invariant. This means w((p g (x)) = D(p g (x) ■ w(x) for all g, x. 

Definition 5: An invariant frame (w±, w n ) on X is a set of n linearly point- wise independent 
G-invariant vector fields, i.e (wi(x), ...,w n (x)) is a basis of the tangent space to X at x. 
We are now going to explain how to build an invariant frame. We follow [15], theorem 2.84 and 
we apply the moving frame method to the following case: E = X, and (cf) g ) geG = ((p g ) geG and 
the normalization equations (p g (x) = c give g = 7(2;). 

Lemma 1: The vector fields defined by 

d 

Wi(x) := (Dip^x))' 1 ■ — , i = l,...,n, (5) 
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where (^-, ^-) is the canonical frame of X, form an invariant framed. 

Proof: They are clearly point-wise linearly independent. Each Wi is invariant because for 
any group element b we have 

. Wi(<p b (x)) = (Dip^^iipbix)))- 1 £: and thus 

{D^bix))- 1 Wi((p b (x)) = [D(p j{ipb{x)) ((p b (x)) Dcp b (x)]~ 

• the group structure implies that, for any group elements c, d, we have <fi c (<fid) (x) = (p cd (x); 
thus 

D(p c ((p d (x)) Dip d (x) = Dip cd (x) 
Thus, with c = ^((fbix)) and d = b, we have 

D{ Pi^ b {x))(Vb(x))Dif b (x) = Dip j{(pb{x) ) b (x); 

• since ^((p b (x))b = ^y(x) (eq ©), we have (corresponding to definition B} 

d 

(Dip b (x)Y l Wi((p b (x)) = (D<£ l{x) (x)Y l — = Wi(x). 

■ 

3) Base and fiber coordinates: We introduce base and fiber coordinates which are useful local 
coordinates to express G-invariant systems of definition We suppose from now on that G is the 
r-dimensional (r < n) group acting on X x hi (see ©) and for each x, the mapping g p g (x) 
is full rank. The moving frame method provides a set of fundamental local invariants Zb G IR n ~ r 
of the group action on X alone. Complete it with z a G W so that (z a , z b ) form coordinates of 
X. These coordinates are called fiber (z a ) and base (z b ) coordinates (see [16]). One can always 
choose z a such that for any g E G the group transformation reads (p g (z a , z b ) = (zu(z a ), z b ) with 
g i — > w g (x) invertible for all x £ W. Let z = (z a , z b ). Let 7 be the moving frame which maps 
z to the coordinate cross-section {z a = c}. The invariant dynamics © writes locally in the new 
coordinates: 

d (6) 
-r t z b = fb(c,z b ,i/; 7{z) (u)) 

since the system is invariant. Example IIII-BI illustrates the interest of such coordinates. 

'One could take any basis (ei, e n ) of X instead of the canonical frame 
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D. Characterization of invariant pre-observers 

Definition 6 (pre-observer): The system j- t x = F(x, u, y) is a pre-observer of ©-© if for 

all x, u F(x, u, h(x, u)) = f(x, u). 

The definition does not deal with convergence; if moreover x(t) — ► x(t) as t — > +00 for every 
(close) initial conditions, the pre-observer is an (asymptotic) observer. 

Definition 7: The pre-observer ^,x = F(x,u,y) is G-invariant if for all g,x,u,y, 

F((f g (x),^g(u),g g (y)) = Dtp g (x) ■ F(x,u,y). 
The property also reads 4tX = F(X, U, Y), with X — <f g (x), U — ip g {u) and Y = g g (y). This 
means the pre-observer remains unchanged under the action of G on each of the three spaces 
X, U, and y via (resp.) ip g , ip g and g g . Obviously we call invariant observer an asymptotic 
G-invariant pre-observer. 

The assumption that the output is G-equivariant is motivated by the following result of [7]: if 
the pre-observer -^x = F(x,u,y) is invariant and if the rank of F versus y is equal to dim(y), 
then, the output map y is G-equivariant in the sense of definition [3l 

In general the "usual" output error y — y = h(x, u)—y does not preserve the system geometry, 
hence it will not yield an invariant pre-observer. The key idea in order to build an invariant (pre-) 
observer is to use, as noticed in [2], an invariant output error instead of the usual output error. 

Definition 8: The smooth map (x,u,y) (—> E(x,u,y) G MP is an invariant output error if 

• the map y 1— ► E(x, u, y) is invertible for all x, u 

• E(x, u, h(x, u)) = for all x, u 

• E(<Pg(?), il> g (u), Q g {y)) = E(x, u, y) for all x, u, y 

The first and second properties mean E is an "output error", i.e. it is zero if and only if h(x, u) = 
y; the third property, which also reads E(X, U, Y) = E(x, u, y), expresses invariance. 

Theorem 1: ^x = F(x,u,y) is a G-invariant pre-observer for the G-invariant system j- t x = 
f(x,u) with G-equivariant output y = h(x,u) if and only if 

n 

F(x,u,y) = f(x,u) + ^2C l (l(x,u),E(x,u,y))w i (x), 

i=i 

where E is an invariant output error, (x, u) 1— > I(x, u) E M. n+m ~ r is a full-rank invariant function, 
the Ci's are smooth functions such that for all x, £i(l(x,u),0) = 0, and (wi, w n ) is an 
invariant frame. 
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Since each £j is smooth and satisfies £j(J, 0) = 0, we can write Ci(I,E) = Ci(I,E) ■ E 
where Ci(I, E) is a p x 1 matrix with entries depending on (J, E). Hence, 

n n 

Y i C i {I,E)w i = £3 ■ E) 



i=l i=l 



W\ ■ ■ ■ w r 



\£n(I,E)J 



E 



The observer can thus be written as 

F(x, u, y) = f(x, u) + W(x)C(l(x, u), E(x, u, y))E(x, u, y) (7) 

where W(x) = (wi(x), .., w n (x)) and C is a n x p matrix whose entries depend on (I,E). 
The observer can be thought of as a gain-scheduled observer with a n x p gain matrix W ■ L 
multiplied by the nonlinear error E. 

Notice the theorem says nothing about convergence but only deals with the structure of the 
pre-observer. 

To prove theorem Q] we first prove the following theorem which ensures the existence of a 
(local) invariant output error. The proof is constructive and relies on the Cartan moving frame 
method (see section Hl-C.il) . 

Theorem 2: We have the three following statements 

• there is an invariant output error (x, u, y) h- > E(x, u, y) 

• there is a full-rank invariant function (x,u) t— > I(x,u) E W n+m ~ r (a complete set of n+m-r 
independent scalar invariants) 

• every other invariant output error reads 

E(x,u,y) = C(l(x,u),E(x,u,y)). 

where C is any smooth function such that C(I, 0) = and E i— > C(I, E) is invertible. 
Proof: We apply the moving frame method (section III-C.ll) to the following case: £ = 
X x U x y, and <f) g is the composite transformation 

(f) g (x,u,y) := (<p g (x),ipg(u),Q g (y)). 
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Since the action of G on X is full rank we can split x i— > <£>t,(x) into ^(x) £ ^ r > which is 
invertible with respect to g, and the remaining part <p b g (x) E IR n ~ r . The r normalization equations 

tp a g {x) = c (8) 

can then be solved and give g = j(x), which can be substituted into the remaining equations to 
yield the complete set of n + m + p — r functionally independent invariants 

I(x,u) := {<P*(&)(x),i>i(&)(u)) (9) 

Jh&y) '= Qi{&) (y)- ( 10 ) 

An invariant output error is then given by 

E(x,u,y) := J h {x,h(x,u)) - Jh(x,y) (11) 

Actually, since it is an invariant function of x, u and y, every invariant output error E must 
have the form 

E(x, u, y) = F(l(x, u), J h (x, y)) 

= F(l(x, u), Jh(x, h(x,u)) - E(x,u,y)) 
= C(l(x,u),E(x,u,y)) 

We used the fact that Jh(x, h(x, u)), which is by construction invariant, must be a function 
of I(x, u) (fundamental invariants of x and u). ■ 
We are now able to give the proof of theorem CD 

Proof: The vector field F in the theorem clearly is a pre-observer. Indeed, 

n 

F(x,u,h(x)) = f(x, u) + Li (l(x, u), E(x, u, h(x))jWi(x) 

8=1 

n 

= f(x, u) + A (J(x, u), 0)wi(x) 

8=1 

= f(x,u) 

By construction, it is invariant. 

Conversely, assume j^x = F{x,u,y) is a G-invariant observer. It can be decomposed on the 
point- wise independent wVs as 

n 

F(x, u,y) = ^2 F i( x ' u > V) w i( x )i 

8=1 
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where the F/s are smooth functions. Since it is a pre-observer, 

n 

f(x, u) = F(x, u, h(x, u)) = Fi(x, u, h(x, u])wi{x). 

8=1 

Since it is a G-invariant pre-observer 

n n 

^Fi(v9 9 (x), ip g (u), Q g {y)) Wi((p g (x)) = D(p g (x) ■^2F i (x,u,y)w i (x) 

i=l i=l 

but the Wi's verify D(p g (x) ■ Wi(x) = Wi(ip g (x)), hence 

Fi(<Pg(x), if) g (u), Q g {y)) = Fi(x,u,y), % = 1, . . .n. 

Therefore, 

u, y) = /(£, w) + it, y) - f(x, u)] 



f(x, u) + it, y) - F (x, it, u)) 



WAX). 



i=l 



The functions u, y)—Fi (x, u, /t(x, u)) are clearly invariant; hence by theorem[2l Fi(x, u, y) — 
Fi(x,u,h(x,u)) = Ci(l(x,u),E(x,u,y)). U 

E. Invariant pre-observer: a constructive method 

The system must be invariant (i.e, unchanged by transformation ©) with equivariant output 
(definition [3]). Thanks to the last theoretic section we can build all symmetry-preserving pre- 
observers: a) Solve the normalization equations d8]). Build an invariant error thanks to (fTT|) . and 
a complete set of scalar invariants I thanks to ©. b) Build an invariant frame thanks to ©. 
c) The general form of invariant pre-observers is given by theorem [Q There is a convenient 
alternative form CD). 

F. Local convergence around an equilibrium 

In this paragraph we will show it is always possible to turn an asymptotic observer with a 

local gain design into an invariant one with the same local behavior (see the chemical reactor 

of section ITlI-B|) . Indeed consider an equilibrium (x,u,y) characterized by f(x,u) = and 

y = h(x,u). Assume that the linearized system around this equilibrium is observable. This 

means that the pair (A, C) is observable where 

df df dh Oh 

A =-(*,«), B = -(x,u), C = -(x,u), D = -(x,u) 
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Consider the following locally asymptotic observer 

—x = f(x,u) + L(y-y) (12) 

where we have chosen the observer constant gain matrix L such that A + LC is a stable matrix. 
In general, such an observer is not invariant. One can build an invariant observer with the same 
linear-tangent approximation, i.e., a locally asymptotic observer of the form © 

= F(x, u, y) = f(x, u) + W(x)C(l(x, u), E(x, u, y))E(x, u, y) 

with 

OF OF OF 

— (x,u,y)=A + LC, — (x,u,y) = B + LD, — (x,u,y) = -L (13) 
ox ou ay 

Let us suggest a possible choice for L in order to satisfy the above conditions on ^ and 

^ at the equilibrium. Since E(x, u, y) = 0, by differentiation versus x, u and y, we have at the 

equilibrium 

dE_ _dE_ c 9E_ _dE_ D 

dx dy ' du dy 

Let V denote the p x p square invertible matrix V — ^(x, w, y). Take for instance the constant 
matrix 

C = -W(x)- l LV- 1 

The choice proposed for L is such that the above conditions (fT3l are fulfilled. We made an 
invariant observer with same local behavior as (fT2l) . 



G. Invariant state-error and convergence issue 

We have no general constructive procedure to design the gain functions £j's of theorem [Din 
order to achieve systematic asymptotic convergence of x towards x for any non-linear system 
possessing symmetries. Nevertheless the way the state estimation error is defined plays a key 
role in convergence analysis. Instead of the linear state-error x — x, we will rather consider the 
following invariant state-error 

rj(x,x) = ip 7 ( x )(x) - ip 7 ( x )(x) 

where ^(x) is defined as the solution of © with respect to g. Notice it is equivalent to choose 
x to make the normalization and consider r/(x, x) = (pjf£)(x) — Lp y ^(x). A remarkable result is 
that the error equation only depends on the trajectory via / (n + m — r scalar invariants): 
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Theorem 3: The dynamics of the invariant state-error r)(x,x) = <p^r x -\(x) — <p^t x -)(x) depends 
only on 77 and scalar invariants depending on x and u: 

j^q = T{r],I(x,u)) 

for some smooth function T and where I(x,u) is defined in theorem |2] 

Proof: The error 77 is an invariant: for all g E G we have r)((p g (x), <p g (x)) = rj(x, x). Thus 
£ t r](ip g {x),ip g (x)) = f t v(x,x), i.e, 

diT]D<f g (x)f(x, u) + d2r]Dip g (x)F(x, u, h(x, u)) 

(14) 

=diT)f(x, u) + d2i]F(x, u, h(x, u)) 

where di denotes the partial differential relative to the 2-th variable. Let a(x , x, u) = x) = 

dxr}f(x,u) + d 2 r]F(x,u,h(x,u)). The equality (fT4l) expresses that cr((p g (x),(pg(x),ip g (u)) = 
a(x, x, u). Since x = ^(x)- 1 (v + ( P'y{x){ x )\ & is an invariant function of the variables (r], x, u). 
Since 77 is an invariant, every invariant function of (77, x, u) (in particular ^77) is a function of 
77, and of a fundamental set of scalar invariants of x and u: I(x,u). ■ 
Such invariant coordinates are not unique. Any invariant function of x, x and u equal to zero 
when x = x can be used as an invariant state-error to analyze convergence. Since it must be a 
function of the complete set of 2n + m — r invariants (/(x, u), (fj^(x)^, it must be a function 
of I(x,u) and of the invariant state-error r)(x,x): 

F{l(x, u),i](x, x)) 

where I(x, u) is a complete set of scalar invariant for the action of G on X x U, and 0) = 
for all /. All examples illustrate the interest of such special coordinates to analyze convergence. 

III. Examples 

A. The non-holonomic car 

Consider a non-holonomic car whose dynamics is the following: 

— x = ucos9, —y = usm9, —9 = uv, h(x,y,9) — (x,y) (15) 

at at at 

where u is the velocity and v is a function of the steering angle. We suppose the output is 

the measurement of the position h(x, y, 9) = (x, y) (using a GPS for instance). 
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The system is independent of the origin and of the orientation of the frame chosen, i.e., it 
is invariant under the action of G = SE(2), the group of rotations and translations. We make 



the identification G 



x S 1 thus any element of G writes (x g ,y g ,9 g 



x S 1 . For any 



(x g , y g , 9 g ) E G the map f( Xg ,y g ,e g ) corresponds to the action on G on the state space M 2 x S 1 : 



X 



\ 



x cos 9 g — y sin 9 g + x g 
x sin 9 g + y cos 9 g + y g 
9 + 9 n 



J 



and tp( Xg , Vg ,e g )(u,v) = 

The dynamics is indeed invariant in the sense of definition [2] Take (x g ,y g ,9 g ) G G and 
(ar, y, 9) e M 2 x S 1 and (u, v) e U = R 2 . Set (p {Xg ,y B ,e g ) {x, V, 0) = (X, Y, 6) and ^( Xg , yg ,e g ) («, v) = 
(U, V) (transformation ©). The dynamics in the new variables reads the same: 



4-X = f/cos9, 4-^ = ^sine, 4-0 
at at at 



UV 



The output function is equivariant in the sense of definition [3] for any x g , y g , 9 g , x and y we 



have Q( Xg , yg ,9 g )(x,y) 



x cos 9 g — y sin 9 g + x g 
x sin 9 g + y cos 9 g + y g 



. We apply method of section III-EI to build an 



invariant pre-observer. 

a) Invariant output error: The normalization equations ( 

x cos 9q — y sin 9 + x = 
x sin 9 + y cos 9 + y = 
9 + 9 = 



write with c = 0: 



hence 



Xq 

yo 



^ — x cos — y sin 



V 



x sin 9 — y cos 9 
-9 



7 



/ 



\ 9 J 
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A complete set of invariants is given by (see HI): I(x,y,9,u,v) = ip. 



(u,v) 



. Let 



(xq, yo, 9) T = 7(x, y, 9) T . An invariant output error writes (see CCD)): 

E = Q(x , yo ,9o)(x,y) - Q(x ,y ,e )(x,y) 

cos 9 — sin 6q \ I x \ ( x \ ( cos 9 — sin 9 
sin6» cos6» / \y \y \sm9 cos6> 







D(p( X) y je) (x,y,6) 



cos 9 sin 9 
— sin 9 cos 9 

b) Invariant frame: To build an invariant frame we apply formula ©. Since (Dip 7 ^ x>y ^(x, y, 9)) 
Dtf^-i^y^x, y, 9) and here 7 _1 (x, y, 9) = (x, y, 9) an invariant frame (iw 1; w 2 , w 3 ) is given by 
the image of the canonical basis of IR 2 x S 1 by D<pu. ty ff\ , i.e, the columns of the matrix 

(l \ /cos 9 - sin 9 0^ 
10 = sin 9 cos 9 = (wi W2 W3 
^0 I J \ 1 , 
and one can notice it corresponds to the Frenet frame. 

c) Invariant pre-observer: Any invariant pre-observer reads (see ©) 

' A / * \ 

w cos # \ I cos # — sin 9 
it sin # + sin 9 cos # £ 

V V 

where £ is a smooth 3x2 gain matrix whose entries depend on the invariant error E but also 
on the invariants I(x, y, 9, u, v). 

d) Error equation: The variable we choose to make the normalization is (x, y, 9) T . The 
invariant state-error thus reads (see (III-GI )): 



-1 



d_ 
~dt 



y 



\ 



cos 9 sin 9 
— sin 9 cos 




(16) 



\ 9 / 



(x\ (x^ 

V 



^ (x — x) cos 9 + (y — y) sin 9 ^ 
— (x — x) sin 9 + (y — y) cos 9 



(17) 



\ 9 J 



V 
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and let us denote by rj = (r] x , r/ y , r] e ) T its coordinates in R 2 xS 1 . Notice the first two coordinates of 
the state error coincide with the invariant output error: (rj x , rj y ) = (E x , E y ). Direct computations 
based on 



UV + + C 32 Tjy) 




^m(cos 8 — cos 8)\ 
u (sin 9 — sin 


yield the following autonomous error equation: 




cos(# + tt/2) sin(# + tt/2) 
-sin(^ + vr/2) cos(# + vr/2) 

in (9 0\ 




d_ 
dt 



w 



' u(l - COS1]g) + (uv + £31^ + £32%)%^ 



V 



u sinr]g - (uv + C 31 r] x + C 32 rj y )rj x 





Indeed the invariant error equation is independent of the trajectory and only depends on the 
relative quantities r/ x , r\ y and 770 as predicted by theorem [3] since here the invariants / are (u,v). 

e) Convergence of the error system: We can here tune the gains so that the error system 
is almost globally asymptotically convergent. The error equation writes: 

^-rj x = u(l - cos T]e) + (uv + C 31 i] x + C 32 rj y )rj y + C n rj x + C 12 % 



d 

dt 
d 

dt. 



rjy = u smr] e - (uv + £ 31 r) x + C^VyjVx + £21^ + £22% 
T)g = C 3l r] x + C 32 rj y 



Take 



/ 



£ 



\u\a 









\u\c 
-ub 



\ 



( 



+ 





uv — ubEy 




ubE y — uv 





\ 



(18) 
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where, a, b, c are positive scalar constants, reminding E y = r\ y the error equation writes 

d_ 

~di 
d 



-j] x = u(l - cosi]e) - \u\arj x 



dt 
d 



rj y = u sin 770 



rje = -ubrjy 



— \u 



crjy 



(19) 



df'° — ly 

Let us suppose J t ^° \u(t)\dt = +00 for all t > 0. Consider the regular change of time scale: 
ds = \u\dt, we have (ej = ±1 is the sign of u) 

d 

ds 
d 

ds 
d_ 

ds 



-//., = ei(l - cosr/ e ) - ar} x 
-r) y = e 1 sin 7) 6 - cr] y 
-rje = -eibri y 



with the following triangular structure: 

d 



d . 

— rie = -c—t] 9 -bsmrje 
ds z ds 

^i] x = ei(l - cosr/ e ) - ar) x 

The first equation is the dynamics of the damped non linear pendulum with the almost globally 
stable equilibrium r] d = 0. The second equation is just a first order stable linear system with 
ei(l — cos 7/0) as source term. Thanks to the notion of invariant state errors defined by (fT7l) we 
proved 

Theorem 4: Consider the system (fT5l) . Assume \u(t)\dt = +00 for all to > 0. The non- 
linear observer 



d 
dt 



y 



u cos §\ ( cos 9 — sin 6 0^ 
sin 9 cos 9 
I) 



u sin 9 

uv 



with 



C 



( 



\ 



\u\a 
—\u\c 
-ub 



( 





ut> — ubEy 




cos ^ sin 9 
— sin 6* cos 9 



ubE y — uv 









is almost globally asymptotically convergent. 
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B. A chemical reactor 

This example illustrates the various definitions of section [II] and the construction of invariant 
pre-observers. As an interesting by-product, we show that invariant pre-observers always produce 
positive estimated concentrations. In theorem |3J we propose a gain design that ensures global 
asymptotic stability. The use of base and fiber coordinates and the notion of invariant error play 
a crucial role in the convergence analysis. We consider the classical exothermic reactor of [4]. 
With slightly different notations, the dynamics reads 





— X^ n 
dt 



X) — k exp 



d 



T = D(t)(T in (t) — T) + cexp 



Ea 
RT 

E A 



X 



(20) 



X + v(t) 



dt ~ ' V RT 

y = T 

where (Ea, R, k, c) are positive and known constant parameters, D(t), T m (t) and v(t) are known 
time functions and D(t) > 0. The available online measure is T: the temperature inside the 
reactor. The parameter X m > 0, the inlet composition, is unknown. The reactor composition X 
is not measured. 

These two differential equations correspond to material and energy balances. Their structure 
is independent of the units: the equations write the same whether they are written in mol/l or in 
kg /I for instance. Let us formalize such independence in terms of invariance. We just consider 
a change of material unit corresponding to the following scaling X i— »■ gX and X m i— > gX m 
with g > 0. The group G is the multiplicative group Take x = (X m ,X, T) as state and 
u = (c, D(t),T m (t),v(t)) as known input. The action on X x U is defined for each g > via 
the (linear) transformations 



^ X™ 
X 

\ T I 



I gX in \ 



lfig(x) 



T 



J 



( c \ 
D 

rpin 

V « / 



4>a(u) 



( c/g \ 
D 

rpin 

V « / 



The dynamics (f20b is invariant in the sense of definition [2l Since y = T is unchanged by G 
(g g (y) = y here), it is a G-equivariant output in the sense of definition [3l We apply method of 
section III-EI to build an invariant pre-observer. 
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a) Invariant output error and complete set of invariants : We choose the second component 
of ifg for the normalization and take as normalizing equation ([8]): gX = 1, i.e. 7(2;) = 1/X. 
Then using (fTT|) the invariant output error is E(x,u,y) = T — y and using © the complete 
set of invariant / is made of the remaining components of (p 1 ,^(x) and ipujt{u): I(x,u) = 
(X in /X,f,cX,D,T in ,v). 

b) Invariant frame: According to ©, an invariant frame is: 

Wl = Xm ^ W2=X 4' W3 = W 
where w\ has been multiplied by the scalar invariant X m / X. 

c) Invariant pre-observer: According to theorem [H invariant pre-observers have the fol- 
lowing structure 



( d_ 

It 



X in = d (l(x,u),T-T^) X in 

jX = D(t)(X in -X)-k exp (-^) X + C 2 (l(x, u),f-T)x (21) 

jf = D(t)(T in (t) - f ) + cexp y-^J X + v(t) + £3 (l(x, «), T - t) 

where the £,'s are smooth scalar functions such that 0) = 0. Any invariant observer 
preserves the fact that X and X m are positive quantities. Indeed the domain {(X m , X , T) £ 
IR 3 I X m > 0,X > 0} is positively invariant for (|2TT) . whatever the choices made for £ l5 £ 2 
and £ 3 (£)(t) > 0). 

d) Convergence around an equilibrium: Assume that around a steady-state (X m , X , T) 
of (f20l> . we designed the three constant gains Li, L 2 , and L 3 , such that 

— X in — Li(T — T) 
at 

jX = D(t)(X m - X) - fcexp X + L 2 (f - T) 

d -T = D(t)(T m (t) - T) + cexp (-^i) X + v{t) + L 3 (f - T) 



dt V in 
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is locally convergent around (X m , X , T). Then following the procedure of subsection III-FL we 
get the invariant observer 



d 
dt 



X in — Li(T — T) 



X 1 
X 1 



X = D(t)(X in - X) - hex? 



d 
dt 

^-f = D{t){T in {t) - f) + cexp 



E, 



RT 
_Ea 
RT 



X + L 2 (f-T)j 
X + v{t)+L 3 (f-T) 



that exhibits identical performances around the steady-state. Moreover it provides automatically 
positive estimations for X and X'"\ and the performances are independent of the choice of units. 

e) Invariant error and global convergence of the observer: As the dimension of G is strictly 
smaller than the dimension of X it is interesting to write the dynamics with the base and fiber 
coordinates of section HI-C.3I which are globally defined on the physical domain {(X m ,X, T) £ 
M 3 | X' m > 0,X > 0}. Consider the following change of variable: 

f V \ ( ry _ \ 



\ 



X 

X in 
T 



\ 



Z = log(X) 
£ = \og(X/X m 
T 



J 



Indeed X corresponds to fiber coordinate and X/X m ,T to base coordinates. We took the log 
of these quantities so that the computation of time derivatives is easier. The dynamics (1201) now 
writes: 



j t Z = £>(«p(-£) - 1) 



d 
It 
d 



£ = £>(exp(-£) - 1) 



/cexp 
/cexp 



—T = D(T m - 
dt v 

and the invariant observer (T2TI) writes 

d 



T) + c exp Z exp 



Ea 
RT 

Ea 
RT 

_Ea 
RT 



+ v(t) 



dt 
d 



Z = D(exp(— £) — 1) — /cexp 



— £ = D(exp(-0 - 1) - /cexp 



dt 



f = D(T in - f ) + cexp Z exp 



Ea 
RT 
Ea 
RT 
_Ea 
RT 



-c 2 

+ v(t) + C 3 
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Consider the following gain design ((3 > and k > are two arbitrary parameters) 

C 2 = -pcexpZexp (^-J^J {T-T) + kexp (~^|J ~kexp 
£ 2 — £i = kexp ( — — 4 ) — /cexp 



RTj V ^ 

£ :; j -/vrcxp j J (T — T) — cexp ^ 



+cexp ( ) ) expZ + D(T - T) 



I I exn Z 4- D(f-T) 
RT \ t 

The choice of such non-linear gains ensure global asymptotic stability when there exists M and 
a > such that the measurements verify for allt > 0, M > X m , D(t),T(t) > a. It implies (see 
(HI ) there exists a > such that t > 0, M > X in , X(t), D(t),T(t) > a. The design, although 
specific to the example relies on the notion of invariant state error (see subsection III-GI ). Since 
the normalizing equation © is: gX = 1, i.e. 7(2;) — 1/X the invariant state-error writes in the 
new variables 77 = (Z, £, T) where 

Z = Z -Z = \og(X/X) 

£=£-£ = log(X/X m ) - log(X/X m ) 

f = f-T 

The dynamics of the invariant state error is the following: 

^Z = D(exp(-£) - exp(-f)) - ficexp + z \ exp Z f 

d ~ ( E A \ . ( E A \ 

—T = cexp I -— + Z j (exp Z - 1) - /tcexp I -— + Z j exp Z T 

Since M > D(t) > a, we have lim^ +oc (^(t) — £(£)) = 0, which means the dynamics of the 
system on the base coordinate £ converges independently from its initial value. And the system 
writes: 

^Z = -pcexp + Z^j expZ f + e x (*) 

Jt f = C6XP (fH + Z ) (6XP ^ " 1} " KC6XP (" + Z ) 6XP ^ f ' 
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where ei(t) = _D(exp(— £(£))— exp(— £(£))) and we know that lim tl ^ +00 e\(t) = and j \ei(t)\dt < 
oo. Consider the regular change of time scale r = J* cexp ^— + rfs. Then: 

V = -/5expZ T + e(t) 

-p- = (exp Z — 1) — k exp Z T 

GST 

where e(t) = cXexp (|^ - Z) ei(f). Take V = Z + exp(-Z) + |T 2 as Lyapounov function. 
f t V < \e(t)\(l + 2V). Thus V is bounded and so are the trajectories. Let (Z(t),T(t)) be a 
trajectory. Take U(t) = V(Z,f,t) - e(r)(l - exp(-Z(r))dr. |C/ = -/?KT 2 expZ < 0. A 
standard application of Barbalat's lemma shows that (0, 0) is globally asymptotically stable. 
Guided by invariance considerations, we have obtained the 

Theorem 5: Consider the system (|20l) . Assume there exist M and a > such that for all 
t > 0, M > X in , D(t),T(t) > a. Then for any /3, k > the following non-linear observer: 

|x- = -/?exp(-jy (f-T(t)) cXX- 

|x = D(t)(X m -X)- exp (-^y) (k + f3(T - T(t))cA>) X 

jT = exp (-^y) (l - <f - T(t))) cX + D(t)(T m (t) - T(t)) + v(t) 
is globally converging. 

C. Velocity-aided inertia! navigation 

In low-cost navigation systems, the relatively inaccurate gyroscopes and accelerometers are 
"aided" by velocity measurements (given by an air-data system or a Doppler radar) and magnetic 
sensors. The various measurements are then "merged" according to the (flat-Earth) motion 
equations of the aircraft, usually by a gain- scheduled observer or an extended Kalman filter. The 
convergence analysis, hence the tuning, of such an observer is far from easy. Using our theory, 
we derive in this section a simple invariant observer, which yields an error equation independent 
of the trajectory of the aircraft. The tuning of the gains to achieve local convergence around any 
trajectories is thus straightforward. 

Simulations illustrate the good behavior of the observer even in the presence of noise and 
sensor biases. They moreover indicate that the domain of convergence of the observer with 
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respect to the initial condition should be very large (though we have not investigated the global 
behavior). 

The derivation of the observer and its implementation are strongly simplified when the body 
orientation is described by a quaternion of length 1 (rather than by Euler angles or a rotation 
matrix). 

1) Quaternions: As in [8], we use the quaternion parameterization of SO(3) to derive filters 
for state estimation. The quaternions are a non commutative group. Any quaternion q can be 
written q = q° + q l e\ + q 2 e 2 + q 3 e% with (q°, q 1 , q 2 , q 3 ) e R 4 , the multiplication * is defined by 

e 1 * e± — — 1, e\ * e 2 = —e 2 * e± — e 3 with circular permutations 

and the norm of q is 

V(i ) 2 + (i 1 ) 2 + (i 2 ) 2 + (i 3 ) 2 - An y vector 

p E R 3 can be identified with 

the quaternion p 1 e i + p 2 e 2 + p 3 e 3 . We will make this identification systematically. Then one 
can associate to any quaternion whose norm is 1, a rotation matrix R q e 5*0(3) thanks to the 
following relation: q^ 1 * p * q = R q p for all p. The subgroup of quaternions whose norm is 
1 is denoted by Hi. Conversely, to any rotation R q of 50(3) are associated two quaternions 
±q of length 1. Thus although the state space in the example is 50(3) x IR 3 , we will write 
the elements of 50(3) as quaternions whose norm is 1 (denoted by Hi) and the vectors of 
M 3 as quaternions whose first coordinate is equal to 0. Numerically, quaternions are easier to 
manipulate and compute than matrices in 50(3). The wedge product v x u of vectors of R 3 
writes for the associated quaternions: (v * uo — uo * v )/2. 

2) Motion equations: The motion of a flying rigid body (assuming the Earth is flat and defines 
an inertial frame) is described by 

d 1 

— q = -q * uj 

dt 1 T 

—v = v x u + g" 1 * A grav *q + a (22) 
V = (yv,Vb) = (v,q~ 1 *B*q) 

where (u, a) are inputs and 

• q is the quaternion of norm 1 representing the orientation of the body-fixed frame with 
respect to the earth-fixed frame. Notice the norm of q is left unchanged by the first equation 
because uo is a vector of R 3 (i.e. a quaternion whose first coordinate is 0). 

• u — Ld 1 e\ + uj 2 e2 + uj 3 e3 is the instantaneous angular velocity vector in the body-fixed frame. 
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v 1 ei + v 2 e 2 + v 3 e 3 is the velocity vector of the center of mass in the body-fixed frame 

grav — A grav ei A-grav e 2 ■^ L grav t 



A grav = A 1 d + A 2 e 2 + A 3 e 3 is the gravity vector in the earth-fixed frame. 



• a = a x ei + a 2 e2 + a 3 e3 is the specific acceleration vector, i.e, the aerodynamics forces 
divided by the body mass. 

• B = B 1 ei + B 2 e 2 + B 3 e 3 is the earth's magnetic field expressed in the earth-fixed frame. 
A grav and B are constant over the flying area. The first equation describes the kinematics 
of the body, the second is Newton's force law. The measurements are u>(t), a(t), v(t) and 
<7 _1 (£) * B * q(t) (measured by gyroscopes, accelerometers, air data system or Doppler radar and 
magnetic sensors). Their coordinates are known in the body-fixed frame. The goal is to estimate 
q and v (i.e make a filter and an estimator for q since it is not measured, and a filter for v). 

3) Invariance of the motion equations: From physical consideration, the symmetries are 
associated to the group SE(3) which consists of rotations and translations in dimension 3. 
We identify (up to the multiplication group { — 1, +1}) SE(3) and the state space X = Hi x M 3 . 
For any (q g ,v g ) G G, the map <P(q g ,v„) corresponds to the following action of G on X: 




Q* Qg 
q~ 1 *v*q g + v g 



Here u = I I . For any (q g ,v g ) G G, the map i>( qg , Vg ) is given by 



% 1 * a * Qg ~ v g x 1 * w * Qg) 
-l 



Qg *W*Qg 

Let us verify that the dynamics is invariant in the sense of definition [2l Take (q g ,v g ) G G and 
(q, v) G G and (a, u) G U. Set (transformation ©) 

d n> 1 1 -i n 

j t Q = ^Q * u * Qg = 2 q * q 9 * % *u*Qg= ^Q*^ 

—V = q~ l (v x uj + * A 9 . ra „ * g + a) * q g 

= (V - v g ) x tt + Q' 1 * A grav * Q + A + v g x (q^ *uj*q g ) 
= V xtt + Q- 1 * A grav *Q + A 



7 February 2007 



DRAFT 



24 



thus the dynamics in the new variables reads the same: it is invariant in the sense of definition 
[2l The output function 

V = (.Vv,Vb) = (v,q~ X * B * q) 
is G-equivariant in the sense of definition [3] with 

(Y v , Y b ) = Q( gg , Vg )(y v , y b ) = (q^ 1 * y v * q g + v g , q' 1 *y b * q g ). 

4) An invariant pre-observer: We apply method of section III-EI to build an invariant pre- 
ob server. 

a) Invariant output error and complete set of invariants: The normalization equations ([8]) 
write 

q * <?o = i 

q l *v*q + v = 

(where 1 is the unit quaternion: 1 + Oei + 0e 2 + 0e 3 ), hence 

-l / 

v ° ) \ v I \—q*v* q" 1 

Using ©, a complete set of invariants is given by 

(u \ ( q * lo * q^ 1 
= 
a I \ q * (a + v x u) * q 

Let (qo,v ) be 7(5, v). Using (fTT|) . an invariant output error is given by: 

(q * (v — v) * q^ 1 
B - q * y b * q 

b) Invariant frame: In order to make an invariant frame we must take a basis of the tangent 
space to the identity element. The tangent space to the space of quaternions whose norm is 1 
is the 3-dimensional set of all quaternions whose first coordinate is equal to 0. Let e±, 62, 63 
be the canonical basis of that space, which can be identified with the canonical basis of M 3 . 
We apply formula © with x = (q,v). Since (D(p^ q ^ (q, v)) 1 = D(p^-i^ v -j(q,v) and here 
7~ 1 ((?, v) = (q, v), an invariant frame is given by the set of 6 vector fields whose values in (q, v) 
are the following 

D(p( q , v )(q,v) j^l = ( ' * ] , D(p M (q,v) 

Kt<3 \ V Ki<3 \" " "/ Ki<3 






1 -1 


Ui * q\ 






V J 




l<i<3 












\e,j 


1 =1 


\q 1 *e i *qJ 




l<i<3 
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c) Invariant pre-observer: According to theorem (OQ) any invariant pre-observer reads 



d 1 3 
—q = -q * u + ^ E) ei*q 



dt x 2 
d_ 



i=l 



V = V X OJ 



+ q 1 * A 9rai , * q + a + £• (J, £) g 1 * a * q, 



i=l 



where the £?, are smooth functions of / and E such that £| (J, 0) = and jQ(J, 0) = 0. To put 
it into the alternative form © we decompose E into (E v ,Eb) = (q*(v — v)*q~ 1 ,~B — q*y b *q~ 1 ) 
and write 

£?(!, E v , E b ) = C%{I, E v , E b ) ■ E v + £*.(!, £„, £ 6 ) • £ 6 
E v , Eb) = C v v i {I, E v , E b ) ■ E v + C\ }i {I, E v , E b ) ■ E b , 
where the C q v ^ C q bil C v v i , C v bi are 3 x 1 matrices with entries depending on (I,E v ,E b ). Hence, 

ei*q= {Y,ei{^I,E v ,E h ).E v + ^{I,E v ,Eb)-E b ) J *g 

i=l \i=l / 

J2e i (£l i -E v + Cl..E b ) )*q 



»=i 



V 



y-v,3 ) 



E v + 



C q 



Eb 



* q 



J 



Proceeding in the same way with the other correction term 5^ 3 =1 >Q (I, E) q 1 *e i *q, the general 
invariant pre-observer finally reads 



j f q = \q * w + + £^ 6 ) * g 



w = t)xw + g * A grcro * g + a + q * (C V V E V + C v b E b ) * q 



(23) 
(24) 



where C%, C\, L v v and C b , are 3 x 3 gain matrices whose entries depend on the invariant errors 
E v and E b and also on the invariants I(q, v, a, u). 

As a by-product of the geometric structure of the observer, we automatically have the desirable 
property that the the norm of q is left unchanged by (|23l) . because uj and ClE v + C q b E b are vectors 
of IR 3 (i.e. quaternions with a zero first coordinate). 
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d) Error equation: The invariant state-error as defined in section HTGl reads r\ = (pjr qiV \(q, v) — 
iPf(q,v)(q, v )- One can write rj = (rj g , Vv) where r\ q = q* q^ 1 — 1 and rj v = q * (v — v) * g _1 .But 
here the state space can be looked at as the group SE(3) itself so we will consider the equivalent 
state-error 

r] q = q* q' 1 , r] v = q * (v - v) * q~ x . 

so that 7] is an error in the sense of group multiplication. Thus a small error corresponds to 
(Vq,Vv) close to the group unit element (1,0). Its time derivative verifies: 

-gHq = {\§ * W + (£ q v E v + £>lE b ) *q)*q~ 1 -q* (q' 1 * ^q * U * q' 1 ) 

= + ip v E v + L\E h ) * Vq 
jj.Vv = ?*((i-«)xw + f 1 * A grav *q-q~ 1 * A grav * q 

+ q^ 1 * {£ V V E V + C' b E b ) * q) * q^ 1 + ^q * uj * (v — v) * q^ 1 — q * (v — v) * u * q^ 1 

= q * (v — v) x lu * q^ 1 + q * to x (v — v) * q~ x + Vq 1 * A grav * r\ q 

- A grav + r/" 1 * {C V V E V + C v b E b ) * Vq 

= r]' 1 * A grav *r] q - Agra, + Vg 1 * {£ V V E V + C v b E b ) * ri q 



where E v = i] q * r\ v * r q q 1 and Ef, = B — r\ q * B * r\ q 1 . Thus the error system is autonomous: 

j t r, q = [Cl( Vq * Vv * r]- 1 ) + C q b (B - Vq *B* r]' 1 )] * Vq (25) 

Jjjlv = Vq 1 * [&grav + * Vv * Vq 1 ) + Q( B ~ Vq * B * Vq 1 )] * Vq ~ ^grav 

It does depend neither on the trajectory, nor on the inputs u(t) and a(t). In the general case (see 
theorem [3]) 4^ is a function of v an d /. But here it does not even depend on /. 

e) Convergence of the linearized error system: Let us suppose q and v are close to 
respectively q and v. First order approximations write 8E V = 5v v an d 5E b = — §r] q *~B + ~B*5vq = 
2B x Svq- Thus the linearized error equation writes: 

j t 5v q = C«5 Vv + 2£l(B x 5 Vq ) 

^-Svv = 2A grav x 5vq + C v JVv + 2C v b (B x Srf q ) 
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In (Earth-fixed) coordinates, 
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The matrices were chosen so that the error system decomposes in four decoupled subsystems: 
• the longitudinal subsystem 








-2a, 



■grav 




(26) 



the lateral subsystem 





1-1 







2a, 



■grav 




(27) 



the vertical subsystem 



the heading subsystem 



1— 



XB (B Srjg - B'M) - \((B^Y + (B ) )5r] t 



(28) 



(29) 



We can freely assign the eigenvalues of each of the subsystems. Guided by invariance consid- 
erations we obtained the following non trivial result: 



7 February 2007 



DRAFT 



28 



Theorem 6: Consider the dynamics (|22|) . The non-linear observer 

^J=\q*u J {t) + (ClE v + ClE h )*q 

at z (3Q) 

—v = vx ^(t) + q~ l * A grav *q + a(t) + q^ 1 * (C V V E V + C v b E b ) * q 

with 

E v = q * (v - y v (t)) * q' 1 , E b = B - q * y h {t) * q' 1 

and with the constant gain matrices C%, L v v , L\ and L\ chosen such that the linear systems 
(|26|) . (|27T) . (|28T) and (|29|) . are asymptotically stable, converges locally and exponentially around 
any system trajectory. The invariant estimation state error obeys an autonomous differential 
equation (1251) . The convergence behavior and Lyapounov exponents are completely independent 
of the system trajectory and of the inputs. 

Simulations below indicate that the convergence is far from being only local. We suspect 
much stronger stability. We conjecture that such non-linear invariant observer is almost globally 
convergent. It can not be globally convergent because of the following "spin" effect: if (r) q = 
l,r] v = 0) is a locally asymptotically stable steady state for the invariant error equation (|25l) . 
(r) q = —l,rj v = 0) is also a locally asymptotically stable steady-state. From a physical point of 
view this is not important since q and — q correspond to the same rotation Rg in £0(3). 

5) Simulations: To obtain realistic values of cu, v, a and q^ 1 * B * q all expressed in the body 
frame, we generated a trajectory of a simplified VTOL-like aircraft. The flight is modeled the 
following way: initially q is the unit quaternion. Let k denote the downwards vertical axis of the 
body frame (quaternion e 3 ) and P the position of the center of mass of the body. We suppose 
the motion is such that k is always collinear to P — A grav . We suppose q corresponds to the 
rotation which maps A grav to k and whose rotation axis is collinear to A grav x k. 

We suppose initially that P(0) = P(0) = -P(O) = 0. P(t) follows a circular trajectory whose 
radius is 5 meters, parameterized by the angle 6{t). The function 1 1— ► P{t) is C 3 with 

. For < t < ti we have < 6 < vr/2 and 6(t) = c(l - cos(27rt/ti)) where c = p-^TT is 

chosen such that t\ = 2 s. 
. For ti < t < t 2 we have vr/2 < < 3vr/2 and 0{t) = with t 2 = 4.15 s. 
. For t 2 < t < t 3 we have 3vr/2 < < 2n and 0(t) = -c(l - cos(27r(t - t 2 )/h) with 

t 3 = 6.15 s. 



1 February 2007 



DRAFT 



29 



The drone eventually stops after having followed a circle. The maximum horizontal acceleration 
is approximately 10 ms~ 2 . Such inverse kinematic model provides realistic values for alt), uj(t), 
v(t) and q~ l (t) * B * q{t) corresponding to this trajectory. We take B = [4g, 0, -^\ T ■ 
For the simulations illustrated by figures (I1I2I3I) . the initial conditions are : 



True system Observer (1301) 

go 1 cos(7r/3) 

qi sin(7r/3)/v^3) 

g 2 -mn(7r/3)/vT3) 

q 3 sin(7r/3)/vT3) 

vt 10 

v 2 -10 

v 3 5 

That means the initial rotation differs from the true one up to a 2n/3 angle. The gains are the 
following: M 12 = M 21 = 0.4, N u = N 22 = 4, N 33 = 2 and A = 4. With a grav = 10 ms~ 2 , 
the poles of the longitudinal and the lateral subsystems are: —2(1 ± i) s' 1 and the poles of the 
vertical and heading subsystems are: —2 s _1 . 

The measured signals are noisy and biased: some high frequencies and some bias are added 
to the signals a(t), u(t), v(t) and q^ 1 * B * q(t) in order to represent the imperfections of 
the sensors. The noisy and biased signals are defined by: a m (t) = a(t) + 0.5 [1, —1, 1] + a%, 
and u m {t) = u{t) + 4vr/360 [1, -1, 1] + 0.25 a 2 , and v m {t) = v{t) + 0.5 [1, -1, 1] + a 3 , and 
Ubmit) = yb(t)+0.05 [1, —1, 1]+0.1 cr 4 , where the a- L are independent normally distributed random 
3-dimensional vectors with mean and variance 1, and a(t), u(t), v{t), q^ 1 * B * q(t) are the 
perfect and smooth signals calculated from the VTOL-type drone dynamics. These simulations 
show that the asymptotic observer (l30l) admits a large attraction region and is quite robust to 
measurement noise and bias. 

IV. Conclusion 

A theory of symmetry-preserving observers has been developed. It is mainly composed of: 
a constructive method to find all the symmetry-preserving preobservers (see section III-E|) . and 
a constructive method to find an invariant error between the actual state of the system and its 
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Fig. 1. measured signals (with noise and bias): specific acceleration a , velocity y v = v , normalized magnetic field yt, and 
angular velocity u) in the body frame 



estimate (see section III-GI) . The resulting invariant error equation simplifies the convergence 
analysis. Although we have only provided examples to support these claims, the following 
properties of a symmetry-preserving observer can be expected: 

• The observer naturally inherits important geometric features of the system (e.g. the observed 
concentrations in example Illl-Bl are positive, the observed quaternion in example IlII-CI has 
unit norm). 

• Constant gains can be chosen thanks to the usual linear techniques (see section HI-FI) to 
achieve local congergence. If there are enough symmetries one can expect local convergence 
around every trajectory of the system, and not only around its equilibrium points or "slowly- 
varying" trajectories. 

• As the observer respects the geometry of the system, the global behavior tends to be better 
and the region of attraction larger (compared e.g. to a Luenberger observer). 

Moreover we believe the invariance property of such an observer is often desirable from an 
engineering point of view, if not from an aesthetic one. Finally the method presented in this 
paper can at least be seen as a useful new tool in the not-so-full toolbox of design methods for 
nonlinear observers, since many physical and engineering systems exhibit symmetries. 
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Velocity in the body frame, true, and estimated 
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Fig. 2. velocity v (solid line) and estimated velocity v (dashed-line) via the invariant observer d30l (with noise and bias). 



Quaternion of attitudes, true, and estimated 




time (s) 
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time (s) 



Fig. 3. orientation q (solid line) and estimated orientation q (dashed-line) via the invariant observer J30t (with noise and bias). 
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